//Port of JAMPACK's householder implementation. 
using System;

namespace dnA.Math {
	internal class Householder {
		private Householder(){}
		public static ComplexDoubleVector GenerateColumn(ComplexDoubleMatrix A,  int r1, int r2, int c){
			int ru = r2-r1+1;
			ComplexDoubleVector u = new ComplexDoubleVector(r2-r1+1);

			for (int i=r1; i<=r2; i++){
				u[i-r1] = A[i,c];
				A[i,c] = ComplexDouble.Zero;
			}               

			double norm = u.GetNorm();

			if (r1 == r2 || norm == 0){
				A[r1,c] = new ComplexDouble(-u[0]);
				u[0] = System.Math.Sqrt(2);
				return u;
			}

			ComplexDouble scale = new ComplexDouble(1/norm,0);

			ComplexDouble t = ComplexDouble.Zero;
			ComplexDouble t1 = ComplexDouble.Zero;
			if (u[0].Real != 0 || u[0].Imag != 0){
				t = u[0];
				t1 = ComplexMath.Conjugate(u[0]);
				t = ComplexMath.Absolute(t);
				t = t1/t;
				scale = scale * t;
			}
			A[r1,c] = -ComplexDouble.One/scale;

			for (int i=0; i<ru; i++){
				u[i] = u[i] * scale;
			}

			u[0] = new ComplexDouble(u[0].Real + 1, 0);
			double s = System.Math.Sqrt(1/u[0].Real);

			for (int i=0; i<ru; i++){
				u[i] = new ComplexDouble(s*u[i].Real,s*u[i].Imag);
			}
			return u;
		}

		public static ComplexDoubleVector GenerateRow(ComplexDoubleMatrix A,  int r, int c1, int c2){
			int cu = c2-c1+1;
			ComplexDoubleVector u = new ComplexDoubleVector(cu);

			for (int j=c1; j<=c2; j++){
				u[j-c1]= A[r,j];
				A[r,j] = ComplexDouble.Zero;
			}               

			double norm = u.GetNorm();

			if (c1 == c2 || norm == 0){
				A[r,c1] = new ComplexDouble(-u[0].Real,-u[0].Imag);
				u[0] = System.Math.Sqrt(2);
				return u;
			}

			ComplexDouble scale = new ComplexDouble(1/norm);

			ComplexDouble t = ComplexDouble.Zero;
			ComplexDouble t1 = ComplexDouble.Zero;
			if (u[0].Real != 0 || u[0].Imag != 0){
				t = u[0];
				t1 = ComplexMath.Conjugate(u[0]);
				t = ComplexMath.Absolute(t);
				t = t1/t;
				scale = scale * t;        
			}

			A[r,c1] = -ComplexDouble.One/scale;

			for(int j=0; j<cu; j++){
				u[j] *= scale;
			}

			u[0] = new ComplexDouble(u[0].Real + 1);
			double s = System.Math.Sqrt(1/u[0].Real);

			for (int j=0; j<cu; j++){
				u[j] = new ComplexDouble(s*u[j].Real,-s*u[j].Imag);
			}
			return u;
		}

		public static ComplexDoubleMatrix UA(ComplexDoubleVector u, ComplexDoubleMatrix A, int r1, int r2, int c1, int c2, ComplexDoubleVector v){
			if (r2 < r1 || c2 < c1){
				return A;
			}

			if (r2-r1+1 > u.Length){
				throw new ArgumentException("Householder vector too short.","u");
			}

			if (c2-c1+1 > v.Length){
				throw new ArgumentException("Work vector too short.","v");
			}

			for (int j=c1; j<=c2; j++){
				v[j-c1] = ComplexDouble.Zero;
			}

			for (int i=r1; i<=r2; i++){
				for (int j=c1; j<=c2; j++){
					v[j-c1] = new ComplexDouble(v[j-c1].Real + u[i-r1].Real*A[i,j].Real + u[i-r1].Imag*A[i,j].Imag,
						v[j-c1].Imag + u[i-r1].Real*A[i,j].Imag - u[i-r1].Imag*A[i,j].Real);
				}
			}

			for (int i=r1; i<=r2; i++){
				for (int j=c1; j<=c2; j++){
					A[i,j] = new ComplexDouble(A[i,j].Real - u[i-r1].Real*v[j-c1].Real + u[i-r1].Imag*v[j-c1].Imag,
						A[i,j].Imag - u[i-r1].Real*v[j-c1].Imag - u[i-r1].Imag*v[j-c1].Real);
				}
			}
			return A;
		}

		public static ComplexDoubleMatrix UA(ComplexDoubleVector u, ComplexDoubleMatrix A, int r1, int r2, int c1, int c2) {
			if (c1 > c2){
				return A;
			}
			return UA(u, A, r1, r2, c1, c2, new ComplexDoubleVector(c2-c1+1));
		}


		public static ComplexDoubleMatrix AU(ComplexDoubleMatrix A, ComplexDoubleVector u, int r1, int r2, int c1, int c2,  ComplexDoubleVector v){
			if(r2 < r1 || c2 < c1){
				return A;
			}

			if (c2-c1+1 > u.Length){
				throw new ArgumentException("Householder vector too short.","u");
			}

			if (r2-r1+1 > v.Length){
				throw new ArgumentException("Work vector too short.","v");
			}

			for (int i=r1; i<=r2; i++){
				v[i-r1] = ComplexDouble.Zero;
				for (int j=c1; j<=c2; j++){
					v[i-r1] = new ComplexDouble(v[i-r1].Real + A[i,j].Real * u[j-c1].Real - A[i,j].Imag*u[j-c1].Imag,
						v[i-r1].Imag + A[i,j].Real*u[j-c1].Imag + A[i,j].Imag*u[j-c1].Real);
				}
			}
			for (int i=r1; i<=r2; i++){
				for (int j=c1; j<=c2; j++){
					A[i,j] = new ComplexDouble( A[i,j].Real - v[i-r1].Real*u[j-c1].Real - v[i-r1].Imag*u[j-c1].Imag,
						A[i,j].Imag + v[i-r1].Real*u[j-c1].Imag - v[i-r1].Imag*u[j-c1].Real);
				}
			}
			return A;
		}


		public static ComplexDoubleMatrix AU(ComplexDoubleMatrix A, ComplexDoubleVector u, int r1, int r2, int c1, int c2){
			if(r2 < r1){
				return A;
			}
			return AU(A, u, r1, r2, c1, c2, new ComplexDoubleVector(r2-r1+1));
		}

		public static ComplexFloatVector GenerateColumn(ComplexFloatMatrix A,  int r1, int r2, int c){
			int ru = r2-r1+1;
			ComplexFloatVector u = new ComplexFloatVector(r2-r1+1);

			for (int i=r1; i<=r2; i++){
				u[i-r1] = A[i,c];
				A[i,c] = ComplexFloat.Zero;
			}               

			float norm = u.GetNorm();

			if (r1 == r2 || norm == 0){
				A[r1,c] = new ComplexFloat(-u[0]);
				u[0] = (float)System.Math.Sqrt(2);
				return u;
			}

			ComplexFloat scale = new ComplexFloat(1/norm,0);

			ComplexFloat t = ComplexFloat.Zero;
			ComplexFloat t1 = ComplexFloat.Zero;
			if (u[0].Real != 0 || u[0].Imag != 0){
				t = u[0];
				t1 = ComplexMath.Conjugate(u[0]);
				t = ComplexMath.Absolute(t);
				t = t1/t;
				scale = scale * t;
			}

			A[r1, c] = -ComplexFloat.One / scale;

			for (int i=0; i<ru; i++){
				u[i] = u[i] * scale;
			}

			u[0] = new ComplexFloat(u[0].Real + 1, 0);
			float s = (float)System.Math.Sqrt(1/u[0].Real);

			for (int i=0; i<ru; i++){
				u[i] = new ComplexFloat(s*u[i].Real,s*u[i].Imag);
			}
			return u;
		}

		public static ComplexFloatVector GenerateRow(ComplexFloatMatrix A,  int r, int c1, int c2){
			int cu = c2-c1+1;
			ComplexFloatVector u = new ComplexFloatVector(cu);

			for (int j=c1; j<=c2; j++){
				u[j-c1]= A[r,j];
				A[r,j] = ComplexFloat.Zero;
			}               

			float norm = u.GetNorm();

			if (c1 == c2 || norm == 0){
				A[r,c1] = new ComplexFloat(-u[0].Real,-u[0].Imag);
				u[0] = (float)System.Math.Sqrt(2);
				return u;
			}

			ComplexFloat scale = new ComplexFloat(1/norm);

			ComplexFloat t = ComplexFloat.Zero;
			ComplexFloat t1 = ComplexFloat.Zero;
			if (u[0].Real != 0 || u[0].Imag != 0){
				t = u[0];
				t1 = ComplexMath.Conjugate(u[0]);
				t = ComplexMath.Absolute(t);
				t = t1/t;
				scale = scale * t;        
			}

			A[r,c1] = -ComplexFloat.One/scale;

			for(int j=0; j<cu; j++){
				u[j] *= scale;
			}

			u[0] = new ComplexFloat(u[0].Real + 1);
			float s = (float)System.Math.Sqrt(1/u[0].Real);

			for (int j=0; j<cu; j++){
				u[j] = new ComplexFloat(s*u[j].Real,-s*u[j].Imag);
			}
			return u;
		}

		public static ComplexFloatMatrix UA(ComplexFloatVector u, ComplexFloatMatrix A, int r1, int r2, int c1, int c2, ComplexFloatVector v){
			if (r2 < r1 || c2 < c1){
				return A;
			}

			if (r2-r1+1 > u.Length){
				throw new ArgumentException("Householder vector too short.","u");
			}

			if (c2-c1+1 > v.Length){
				throw new ArgumentException("Work vector too short.","v");
			}

			for (int j=c1; j<=c2; j++){
				v[j-c1] = ComplexFloat.Zero;
			}

			for (int i=r1; i<=r2; i++){
				for (int j=c1; j<=c2; j++){
					v[j-c1] = new ComplexFloat(v[j-c1].Real + u[i-r1].Real*A[i,j].Real + u[i-r1].Imag*A[i,j].Imag,
						v[j-c1].Imag + u[i-r1].Real*A[i,j].Imag - u[i-r1].Imag*A[i,j].Real);
				}
			}

			for (int i=r1; i<=r2; i++){
				for (int j=c1; j<=c2; j++){
					A[i,j] = new ComplexFloat(A[i,j].Real - u[i-r1].Real*v[j-c1].Real + u[i-r1].Imag*v[j-c1].Imag,
						A[i,j].Imag - u[i-r1].Real*v[j-c1].Imag - u[i-r1].Imag*v[j-c1].Real);
				}
			}
			return A;
		}

		public static ComplexFloatMatrix UA(ComplexFloatVector u, ComplexFloatMatrix A, int r1, int r2, int c1, int c2) {
			if (c1 > c2){
				return A;
			}
			return UA(u, A, r1, r2, c1, c2, new ComplexFloatVector(c2-c1+1));
		}


		public static ComplexFloatMatrix AU(ComplexFloatMatrix A, ComplexFloatVector u, int r1, int r2, int c1, int c2,  ComplexFloatVector v){
			if(r2 < r1 || c2 < c1){
				return A;
			}

			if (c2-c1+1 > u.Length){
				throw new ArgumentException("Householder vector too short.","u");
			}

			if (r2-r1+1 > v.Length){
				throw new ArgumentException("Work vector too short.","v");
			}

			for (int i=r1; i<=r2; i++){
				v[i-r1] = ComplexFloat.Zero;
				for (int j=c1; j<=c2; j++){
					v[i-r1] = new ComplexFloat(v[i-r1].Real + A[i,j].Real * u[j-c1].Real - A[i,j].Imag*u[j-c1].Imag,
						v[i-r1].Imag + A[i,j].Real*u[j-c1].Imag + A[i,j].Imag*u[j-c1].Real);
				}
			}
			for (int i=r1; i<=r2; i++){
				for (int j=c1; j<=c2; j++){
					A[i,j] = new ComplexFloat( A[i,j].Real - v[i-r1].Real*u[j-c1].Real - v[i-r1].Imag*u[j-c1].Imag,
						A[i,j].Imag + v[i-r1].Real*u[j-c1].Imag - v[i-r1].Imag*u[j-c1].Real);
				}
			}
			return A;
		}


		public static ComplexFloatMatrix AU(ComplexFloatMatrix A, ComplexFloatVector u, int r1, int r2, int c1, int c2) {
			if(r2 < r1){
				return A;
			}
			return AU(A, u, r1, r2, c1, c2, new ComplexFloatVector(r2-r1+1));
		}

		public static FloatVector GenerateColumn(FloatMatrix A, int r1, int r2, int c) {
			int ru = r2 - r1 + 1;
			FloatVector u = new FloatVector(r2 - r1 + 1);

			for (int i = r1; i <= r2; i++) {
				u[i - r1] = A[i, c];
				A[i, c] = 0.0f;
			}

			float norm = u.GetNorm();

			if (r1 == r2 || norm == 0) {
				A[r1, c] = -u[0];
				u[0] = (float)System.Math.Sqrt(2);
				return u;
			}

			float scale = 1.0f / norm;

			if (u[0] < 0.0f)
				scale *= -1.0f;

			A[r1, c] = -1.0f / scale;

			for (int i = 0; i < ru; i++) {
				u[i] = u[i] * scale;
			}

			u[0] = u[0] + 1.0f;
			float s = (float)System.Math.Sqrt(1 / u[0]);

			for (int i = 0; i < ru; i++) {
				u[i] = s * u[i];
			}
			return u;
		}

		public static FloatVector GenerateRow(FloatMatrix A, int r, int c1, int c2) {
			int cu = c2 - c1 + 1;
			FloatVector u = new FloatVector(cu);

			for (int j = c1; j <= c2; j++) {
				u[j - c1] = A[r, j];
				A[r, j] = 0.0f;
			}

			float norm = u.GetNorm();

			if (c1 == c2 || norm == 0) {
				A[r, c1] = -u[0];
				u[0] = (float)System.Math.Sqrt(2);
				return u;
			}

			float scale = 1.0f / norm;
			if (u[0] < 0.0f)
				scale *= -1.0f;

			A[r, c1] = -1.0f / scale;

			for (int j = 0; j < cu; j++) {
				u[j] *= scale;
			}

			u[0] += 1.0f;
			float s = (float)System.Math.Sqrt(1 / u[0]);

			for (int j = 0; j < cu; j++) {
				u[j] *= s;
			}
			return u;
		}

		public static FloatMatrix UA(FloatVector u, FloatMatrix A, int r1, int r2, int c1, int c2, FloatVector v) {
			if (r2 < r1 || c2 < c1) {
				return A;
			}

			if (r2 - r1 + 1 > u.Length) {
				throw new ArgumentException("Householder vector too short.", "u");
			}

			if (c2 - c1 + 1 > v.Length) {
				throw new ArgumentException("Work vector too short.", "v");
			}

			for (int j = c1; j <= c2; j++) {
				v[j - c1] = 0.0f;
			}

			for (int i = r1; i <= r2; i++) {
				for (int j = c1; j <= c2; j++) {
					v[j - c1] = v[j - c1] + u[i - r1] * A[i, j];
				}
			}

			for (int i = r1; i <= r2; i++) {
				for (int j = c1; j <= c2; j++) {
					A[i, j] = A[i, j] - u[i - r1] * v[j - c1];
				}
			}
			return A;
		}

		public static FloatMatrix UA(FloatVector u, FloatMatrix A, int r1, int r2, int c1, int c2) {
			if (c1 > c2) {
				return A;
			}
			return UA(u, A, r1, r2, c1, c2, new FloatVector(c2 - c1 + 1));
		}


		public static FloatMatrix AU(FloatMatrix A, FloatVector u, int r1, int r2, int c1, int c2, FloatVector v) {
			if (r2 < r1 || c2 < c1) {
				return A;
			}

			if (c2 - c1 + 1 > u.Length) {
				throw new ArgumentException("Householder vector too short.", "u");
			}

			if (r2 - r1 + 1 > v.Length) {
				throw new ArgumentException("Work vector too short.", "v");
			}

			for (int i = r1; i <= r2; i++) {
				v[i - r1] = 0.0f;
				for (int j = c1; j <= c2; j++) {
					v[i - r1] = v[i - r1] + A[i, j] * u[j - c1];
				}
			}
			for (int i = r1; i <= r2; i++) {
				for (int j = c1; j <= c2; j++) {
					A[i, j] = A[i, j] - v[i - r1] * u[j - c1];
				}
			}
			return A;
		}


		public static FloatMatrix AU(FloatMatrix A, FloatVector u, int r1, int r2, int c1, int c2) {
			if (r2 < r1) {
				return A;
			}
			return AU(A, u, r1, r2, c1, c2, new FloatVector(r2 - r1 + 1));
		}

		public static DoubleVector GenerateColumn(DoubleMatrix A, int r1, int r2, int c) {
			int ru = r2 - r1 + 1;
			DoubleVector u = new DoubleVector(r2 - r1 + 1);

			for (int i = r1; i <= r2; i++) {
				u[i - r1] = A[i, c];
				A[i, c] = 0.0;
			}

			double norm = u.GetNorm();

			if (r1 == r2 || norm == 0) {
				A[r1, c] = -u[0];
				u[0] = (double)System.Math.Sqrt(2);
				return u;
			}

			double scale = 1.0 / norm;
			if (u[0] < 0.0)
				scale *= -1.0;

			A[r1, c] = -1.0 / scale;

			for (int i = 0; i < ru; i++) {
				u[i] *= scale;
			}

			u[0] += 1.0;
			double s = (double)System.Math.Sqrt(1 / u[0]);

			for (int i = 0; i < ru; i++) {
				u[i] *= s;
			}
			return u;
		}

		public static DoubleVector GenerateRow(DoubleMatrix A, int r, int c1, int c2) {
			int cu = c2 - c1 + 1;
			DoubleVector u = new DoubleVector(cu);

			for (int j = c1; j <= c2; j++) {
				u[j - c1] = A[r, j];
				A[r, j] = 0.0;
			}

			double norm = u.GetNorm();

			if (c1 == c2 || norm == 0) {
				A[r, c1] = -u[0];
				u[0] = (double)System.Math.Sqrt(2);
				return u;
			}

			double scale = 1.0 / norm;
			if (u[0] < 0.0)
				scale *= -1.0;

			A[r, c1] = -1.0 / scale;

			for (int j = 0; j < cu; j++) {
				u[j] *= scale;
			}

			u[0] = u[0] + 1.0;
			double s = (double)System.Math.Sqrt(1 / u[0]);

			for (int j = 0; j < cu; j++) {
				u[j] *= s;
			}
			return u;
		}

		public static DoubleMatrix UA(DoubleVector u, DoubleMatrix A, int r1, int r2, int c1, int c2, DoubleVector v) {
			if (r2 < r1 || c2 < c1) {
				return A;
			}

			if (r2 - r1 + 1 > u.Length) {
				throw new ArgumentException("Householder vector too short.", "u");
			}

			if (c2 - c1 + 1 > v.Length) {
				throw new ArgumentException("Work vector too short.", "v");
			}

			for (int j = c1; j <= c2; j++) {
				v[j - c1] = 0.0;
			}

			for (int i = r1; i <= r2; i++) {
				for (int j = c1; j <= c2; j++) {
					v[j - c1] = v[j - c1] + u[i - r1] * A[i, j];
				}
			}

			for (int i = r1; i <= r2; i++) {
				for (int j = c1; j <= c2; j++) {
					A[i, j] = A[i, j] - u[i - r1] * v[j - c1];
				}
			}
			return A;
		}

		public static DoubleMatrix UA(DoubleVector u, DoubleMatrix A, int r1, int r2, int c1, int c2) {
			if (c1 > c2) {
				return A;
			}
			return UA(u, A, r1, r2, c1, c2, new DoubleVector(c2 - c1 + 1));
		}


		public static DoubleMatrix AU(DoubleMatrix A, DoubleVector u, int r1, int r2, int c1, int c2, DoubleVector v) {
			if (r2 < r1 || c2 < c1) {
				return A;
			}

			if (c2 - c1 + 1 > u.Length) {
				throw new ArgumentException("Householder vector too short.", "u");
			}

			if (r2 - r1 + 1 > v.Length) {
				throw new ArgumentException("Work vector too short.", "v");
			}

			for (int i = r1; i <= r2; i++) {
				v[i - r1] = 0.0;
				for (int j = c1; j <= c2; j++) {
					v[i - r1] = v[i - r1] + A[i, j] * u[j - c1];
				}
			}
			for (int i = r1; i <= r2; i++) {
				for (int j = c1; j <= c2; j++) {
					A[i, j] = A[i, j] - v[i - r1] * u[j - c1];
				}
			}
			return A;
		}


		public static DoubleMatrix AU(DoubleMatrix A, DoubleVector u, int r1, int r2, int c1, int c2) {
			if (r2 < r1) {
				return A;
			}
			return AU(A, u, r1, r2, c1, c2, new DoubleVector(r2 - r1 + 1));
		}


	}
}
